package sensor;
// mina - fing - udelar
// robótica basada en comportamientos 2012
// grupo 5
// laboratorio 0

// parte D - sensor de color original

import lejos.nxt.Button;
import lejos.nxt.ColorSensor;
import lejos.nxt.LCD;
import lejos.nxt.comm.RConsole;
import lejos.robotics.Color;
import lejos.util.Delay;


public class MainColorSensorB {
	// variables globales

	// valors rgb de cada color reconocible.
	// black, red, green, blue, yellow, magenta
	static int colorCant = 2;
    static private int[] [] colorRGB = new int [3] [3];
	static String colorNames[] = {"Blue    ", "Orange   ", "not detected"};
	ColorSensor cs = null;// = new ColorSensor (SensorPort.getInstance(2));

	public MainColorSensorB(ColorSensor color){
		this.cs = color;
	}
	
	// función que calibra el sensor de color.
    public void calibrar() {
		// iniciar sistema
		//ColorSensor cs = new ColorSensor(SensorPort.getInstance(2));
		cs.setFloodlight(Color.WHITE);

		// calibrar low

		LCD.drawString ("Search darkness", 0, 0);
		LCD.drawString ("and press Enter", 0, 1);
		Button.waitForAnyPress();
		// esperar a que se presione enter.
		/*while (! Button.ENTER.isDown()) {
			ColorSensor.Color vals = cs.getColor();

			LCD.drawString ("Red:   " + vals.getRed() + "    ", 2, 3);
			LCD.drawString ("Green: " + vals.getGreen() + "    ", 2, 4);
			LCD.drawString ("Blue:  " + vals.getBlue() + "    ", 2, 5);
			LCD.drawString ("Light: " + cs.getLightValue() + "    ", 2, 7);
		}*/
		
		// calibrar
		cs.calibrateLow();

		// esperar a que se despresione enter.
		LCD.drawString ("Processing... ", 0, 0);
		Delay.msDelay (500);

		// calibrar high
		
		LCD.drawString ("Search light", 0, 0);
		LCD.drawString ("and press Enter", 0, 1);
		Button.waitForAnyPress();
		// esperar a que se presione enter.
		/*while (! Button.ENTER.isDown()) {
			ColorSensor.Color vals = cs.getColor();

			LCD.drawString ("Red:   " + vals.getRed() + "    ", 2, 3);
			LCD.drawString ("Green: " + vals.getGreen() + "    ", 2, 4);
			LCD.drawString ("Blue:  " + vals.getBlue() + "    ", 2, 5);
			LCD.drawString ("Light: " + cs.getLightValue() + "    ", 2, 7);
		}*/
		
		// calibrar
		cs.calibrateHigh();

		// esperar a que se despresione enter.
		LCD.drawString ("Processing... ", 0, 0);
		Delay.msDelay (500);
		
		// reconocer cada color y guardarlos.

		for (int idxColor = 0; idxColor < colorCant; idxColor += 1) {
			LCD.drawString ("Search " + colorNames [idxColor], 0, 0);
			LCD.drawString ("and press Enter", 0, 1);
			
			Button.waitForAnyPress();
			// esperar a que se presione enter.

			/*while (! Button.ENTER.isDown()) {
				ColorSensor.Color vals = cs.getColor();
	
				LCD.drawString ("Red:   " + vals.getRed() + "    ", 2, 3);
				LCD.drawString ("Green: " + vals.getGreen() + "    ", 2, 4);
				LCD.drawString ("Blue:  " + vals.getBlue() + "    ", 2, 5);
				LCD.drawString ("Light: " + cs.getLightValue() + "    ", 2, 7);
			}*/

			// obtener valores rgb y guardarlos en colorRGB.

			ColorSensor.Color vals = cs.getColor();

			colorRGB [idxColor] [0] = vals.getRed();
			colorRGB [idxColor] [1] = vals.getGreen();
			colorRGB [idxColor] [2] = vals.getBlue();

			// esperar a que se despresione enter.
			LCD.drawString ("Processing... ", 0, 0);
			Delay.msDelay (500);
		}
    }


	// función que devuelve el gap entre el color reconocido y el indicado.

    static int calcularGapColor (ColorSensor.Color vals, int idxColor) {
		int gapRed = (vals.getRed() - colorRGB [idxColor] [0]) * (vals.getRed() - colorRGB [idxColor] [0]);
		int gapGrn = (vals.getGreen() - colorRGB [idxColor] [1]) * (vals.getGreen() - colorRGB [idxColor] [1]);
		int gapBlu = (vals.getBlue() - colorRGB [idxColor] [2]) * (vals.getBlue() - colorRGB [idxColor] [2]);
		return gapRed + gapGrn + gapBlu;
	}

	// función que reconoce los colores del sensor.

    public int reconocer(){
		// inicializar pantalla
		LCD.clear();
			cs.setFloodlight (Color.WHITE);


       
			// obtener color
			ColorSensor.Color vals = cs.getColor();
		
			int gapReconocido = calcularGapColor (vals, 0);
			int colorReconocido = 0;

			
			for (int idxColor = 1; idxColor < colorCant; idxColor += 1) {
				int gapIndxColor = calcularGapColor (vals, idxColor);
				if (gapIndxColor < gapReconocido) {
					gapReconocido = gapIndxColor;
					colorReconocido = idxColor;
				}
			}
			
			LCD.drawString("GAP: "+Integer.toString(gapReconocido), 0,0 );
			RConsole.println("GAP: "+Integer.toString(gapReconocido));
			if (gapReconocido > 1500) {//1500
				colorReconocido = 2;
			}

			return colorReconocido;
		
    }
 
}
